#define  _CRT_SECURE_NO_WARNINGS

#include "opencv/cv.h"
#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/nonfree/nonfree.hpp"
#include "opencv2/imgproc/imgproc_c.h"
#include "opencv2/legacy/legacy.hpp"
#include "opencv2/legacy/compat.hpp"

#include <vector>
#include "stdafx.h"
#include "ddraw.h"
#include "xhImgAnaly.h"
#include "xfImgBit16.h"
#include <math.h>

#ifdef _DEBUG
#pragma comment(lib, "..//opencv_lib//opencv_core243d.lib")
#pragma comment(lib, "..//opencv_lib//opencv_imgproc243d.lib")
#pragma comment(lib, "..//opencv_lib//opencv_flann243d.lib")
#pragma comment(lib, "..//opencv_lib//opencv_legacy243d.lib")
#pragma comment(lib, "..//opencv_lib//opencv_nonfree243d.lib")
#pragma comment(lib, "..//opencv_lib//opencv_highgui243d.lib")
#else
/*
#pragma comment(lib, "..//opencv_lib//opencv_core243.lib")
#pragma comment(lib, "..//opencv_lib//opencv_imgproc243.lib")
#pragma comment(lib, "..//opencv_lib//opencv_flann243.lib")
#pragma comment(lib, "..//opencv_lib//opencv_legacy243.lib")
#pragma comment(lib, "..//opencv_lib//opencv_nonfree243.lib")
*/
///*
#pragma comment (lib,"vfw32.lib")
#pragma comment (lib,"comctl32.lib")

#pragma comment(lib, "..//opencv_lib_st//opencv_imgproc.lib")
#pragma comment(lib, "..//opencv_lib_st//opencv_flann.lib")
#pragma comment(lib, "..//opencv_lib_st//opencv_calib3d.lib")
#pragma comment(lib, "..//opencv_lib_st//opencv_features2d.lib")
#pragma comment(lib, "..//opencv_lib_st//opencv_legacy.lib")
#pragma comment(lib, "..//opencv_lib_st//opencv_nonfree.lib")
#pragma comment(lib, "..//opencv_lib_st//opencv_video.lib")
#pragma comment(lib, "..//opencv_lib_st//opencv_ml.lib")
#pragma comment(lib, "..//opencv_lib_st//opencv_highgui.lib")
#pragma comment(lib, "..//opencv_lib_st//libpng.lib")
#pragma comment(lib, "..//opencv_lib_st//libjpeg.lib")
#pragma comment(lib, "..//opencv_lib_st//libjasper.lib")
#pragma comment(lib, "..//opencv_lib_st//IlmImf.lib")
#pragma comment(lib, "..//opencv_lib_st//opencv_core.lib")
//*/
#endif



#include "xfKeyThreshold.h"

xfKeyThreshold::xfKeyThreshold(void)
{
}

xfKeyThreshold::~xfKeyThreshold(void)
{
}


int xfKeyThreshold::CreateKeyPoints(void *pImg,void *pCfg)
{
	zkAnalyKeyPointCfg *pKeyPointCfg = (zkAnalyKeyPointCfg*)pCfg;

	IplImage  *pImgDst  = (IplImage *)pImg;

	stKeyInfo *pKeyInfo = NULL;
	RECT      *pRt      = NULL;

	CvMemStorage *pMemStorage  = NULL;
	CvSeq        *pKeypoints   = NULL;
	CvSeq        *pDescriptors = NULL;

//	CvRect rtROI;

#ifdef DKQ_TEST_KEYPOINT_ERROR
	g_xLogInfo.WriteLog(__FILE__,__LINE__,"top:%d,bottom:%d",pKeyPointCfg->rt[0].top,pKeyPointCfg->rt[0].bottom);
#endif

	int i=0;
//	for(i=0;i<ANALYKEYRTCOUNT;i++)
	{
		pKeyInfo = &(m_tKeyInfo[i]);
		pRt      = &(pKeyPointCfg->rt[i]);

// 		if( pRt->left < 0 || pRt->left >= pRt->right || 
// 			pRt->top  < 0 || pRt->top  >= pRt->bottom  )
// 		{
// 			continue;
// 		}

		pMemStorage = pKeyInfo->pMemStorage;
		if( NULL == pMemStorage )
		{
			pMemStorage           = cvCreateMemStorage(1024*1024);
			pKeyInfo->pMemStorage = pMemStorage; 
		}
		if( NULL == pKeyInfo->pMemStorage )
		{
			return DD_ERR_NEWBUFFER;
		}
		cvClearMemStorage( pMemStorage );

/*		
		rtROI.x = pRt->left;
		rtROI.y = pRt->top;
		rtROI.width  = pRt->right  - pRt->left;
		rtROI.height = pRt->bottom - pRt->top;

#ifdef DKQ_TEST_KEYPOINT_ERROR
		g_xLogInfo.WriteLog(__FILE__,__LINE__,"i:%d,top:%d,bottom:%d,left:%d,right:%d",i,pRt->top,pRt->bottom,pRt->left,pRt->right);
#endif

		cvSetImageROI( pImgDst, rtROI );
*/		
		
		pKeypoints   = NULL;
		pDescriptors = NULL;

		CvSURFParams params = cvSURFParams(400, 0);
// 		params.extended = 0;
// 		params.nOctaves = 8;
// 		params.nOctaveLayers = 4;

		try
		{
			cvExtractSURF( pImgDst, 0, &pKeypoints, &pDescriptors, pMemStorage, params );
		}
		catch (...)
		{
		}

		pKeyInfo->pKeyPoints   = pKeypoints;
		pKeyInfo->pDescriptors = pDescriptors;
	}

//	for(i=0;i<ANALYKEYRTCOUNT;i++)
	{
		pKeyInfo = &(m_tKeyInfo[i]);

		if( NULL !=pKeyInfo->pKeyPoints && NULL != pKeyInfo->pDescriptors )
		{
			return 0;
		}
	}

    return DD_ERR_ANALY_NOTFINDKEY;
}



static inline void 
flannFindPairs( const CvSeq*, const CvSeq* objectDescriptors,
			   const CvSeq*, const CvSeq* imageDescriptors, vector<int>& ptpairs )
{
	int length = (int)(objectDescriptors->elem_size/sizeof(float));

	cv::Mat m_object(objectDescriptors->total, length, CV_32F);
	cv::Mat m_image(imageDescriptors->total, length, CV_32F);

	// copy descriptors
	CvSeqReader obj_reader;
	float* obj_ptr = m_object.ptr<float>(0);
	cvStartReadSeq( objectDescriptors, &obj_reader );
	for(int i = 0; i < objectDescriptors->total; i++ )
	{
		const float* descriptor = (const float*)obj_reader.ptr;
		CV_NEXT_SEQ_ELEM( obj_reader.seq->elem_size, obj_reader );
		memcpy(obj_ptr, descriptor, length*sizeof(float));
		obj_ptr += length;
	}
	CvSeqReader img_reader;
	float* img_ptr = m_image.ptr<float>(0);
	cvStartReadSeq( imageDescriptors, &img_reader );
	for(int i = 0; i < imageDescriptors->total; i++ )
	{
		const float* descriptor = (const float*)img_reader.ptr;
		CV_NEXT_SEQ_ELEM( img_reader.seq->elem_size, img_reader );
		memcpy(img_ptr, descriptor, length*sizeof(float));
		img_ptr += length;
	}

	// find nearest neighbors using FLANN
	cv::Mat m_indices(objectDescriptors->total, 2, CV_32S);
	cv::Mat m_dists(objectDescriptors->total, 2, CV_32F);
	cv::flann::Index flann_index(m_image, cv::flann::KDTreeIndexParams(4));  // using 4 randomized kdtrees
	flann_index.knnSearch(m_object, m_indices, m_dists, 2, cv::flann::SearchParams(64) ); // maximum number of leafs checked

	int* indices_ptr = m_indices.ptr<int>(0);
	float* dists_ptr = m_dists.ptr<float>(0);
	for (int i=0;i<m_indices.rows;++i) {
		if (dists_ptr[2*i]<0.15*dists_ptr[2*i+1]) {
			ptpairs.push_back(i);
			ptpairs.push_back(indices_ptr[2*i]);
		}
	}
}


static inline void cvmSet6(CvMat* m, int row, int col, float val1, float val2, float val3, float val4, float val5, float val6)
{
	cvmSet(m, row, col, val1);
	cvmSet(m, row, col + 1, val2);
	cvmSet(m, row, col + 2, val3);
	cvmSet(m, row, col + 3, val4);
	cvmSet(m, row, col + 4, val5);
	cvmSet(m, row, col + 5, val6);
}

static inline void FindAffineTransform(const vector<CvPoint2D32f>& p1, const vector<CvPoint2D32f>& p2, CvMat* affine)
{
	int eq_num = 2*(int)p1.size();
	CvMat* A = cvCreateMat(eq_num, 6, CV_32FC1);
	CvMat* B = cvCreateMat(eq_num, 1, CV_32FC1);
	CvMat* X = cvCreateMat(6, 1, CV_32FC1);

	for(int i = 0; i < (int)p1.size(); i++)
	{
		cvmSet6(A, 2*i, 0, p1[i].x, p1[i].y, 1, 0, 0, 0);
		cvmSet6(A, 2*i + 1, 0, 0, 0, 0, p1[i].x, p1[i].y, 1);
		cvmSet(B, 2*i, 0, p2[i].x);
		cvmSet(B, 2*i + 1, 0, p2[i].y);
	}

	cvSolve(A, B, X, CV_SVD);

	cvmSet(affine, 0, 0, cvmGet(X, 0, 0));
	cvmSet(affine, 0, 1, cvmGet(X, 1, 0));
	cvmSet(affine, 0, 2, cvmGet(X, 2, 0));
	cvmSet(affine, 1, 0, cvmGet(X, 3, 0));
	cvmSet(affine, 1, 1, cvmGet(X, 4, 0));
	cvmSet(affine, 1, 2, cvmGet(X, 5, 0));

	cvReleaseMat(&A);
	cvReleaseMat(&B);
	cvReleaseMat(&X);
}

int xfKeyThreshold::GetAdjustThresholds(void *pRefImgBit,void *pCfg,float *pfThM)
{
	zkAnalyAdjustCfg *pAdjustCfg = (zkAnalyAdjustCfg*)pCfg;
	xfImgBit16       *pRefBit    = (xfImgBit16*)pRefImgBit;

	stKeyInfo *pKeyInfo    = NULL;
	stKeyInfo *pRefKeyInfo = NULL;

	try
	{
		int i=0,j=0;
		for(i=0;i<ANALYKEYRTCOUNT;i++)
		{
			pKeyInfo    = &(m_tKeyInfo[i]);
			pRefKeyInfo = &(pRefBit->m_tKeyInfo[i]);

			if( NULL == pKeyInfo->pKeyPoints ||
				NULL == pKeyInfo->pDescriptors ||
				NULL == pRefKeyInfo->pKeyPoints ||
				NULL == pRefKeyInfo->pDescriptors )
			{
				continue;
			}

			if( pRefKeyInfo->pDescriptors->total <= 0 || 
				pKeyInfo->pDescriptors->total <= 0 
			   )
			{
				return DD_ERR_ANALY_NOTFINDKEY;
			}

			m_arPairs.clear();
			m_arRefPoint.clear();
			m_arSapPoint.clear();

#ifdef DKQ_TEST_TEMP_TIME
			g_xLogInfo.WriteLog(__FILE__,__LINE__,"flannFindPairs -----");
#endif


			flannFindPairs( pRefKeyInfo->pKeyPoints,pRefKeyInfo->pDescriptors, pKeyInfo->pKeyPoints,pKeyInfo->pDescriptors, m_arPairs );


#ifdef DKQ_TEST_TEMP_TIME
			g_xLogInfo.WriteLog(__FILE__,__LINE__,"flannFindPairs ++++++");
#endif

			for( j=0;j<(int)m_arPairs.size(); j += 2 )
			{
				CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem( pRefKeyInfo->pKeyPoints, m_arPairs[j]   );
				CvSURFPoint* r2 = (CvSURFPoint*)cvGetSeqElem( pKeyInfo->pKeyPoints   , m_arPairs[j+1] );

				if(( abs(r1->pt.x-r2->pt.x)<r1->size*3)&&( abs(r1->pt.y-r2->pt.y)<r1->size*3)/*&&abs(r1->hessian-r2->hessian)<10&&abs(r1->size-r2->size)<2*/)
				{
					m_arRefPoint.push_back( r1->pt );
					m_arSapPoint.push_back( r2->pt );
				}
			}
			if( m_arSapPoint.size() < 5 )
			{
				continue;
			}

#ifdef DKQ_TEST_TEMP_TIME
			g_xLogInfo.WriteLog(__FILE__,__LINE__,"aaaa ++++++");
#endif

			CvMat M = cvMat (2, 3, CV_32F, pfThM);

			FindAffineTransform( m_arSapPoint,m_arRefPoint,&M);

#ifdef DKQ_TEST_TEMP_TIME
			g_xLogInfo.WriteLog(__FILE__,__LINE__,"bbbb ++++++");
#endif

			return 0;
		}

	}
	catch (...)
	{
	}

	return -1;
}